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Abstract 

A deterministic attitude estimation problem for a rigid body in a potential field, with 
bounded attitude and angular velocity measurement errors is considered. An attitude esti- 
mation algorithm that globally minimizes the attitude estimation error is obtained. Assum- 
ing that the initial attitude, the initial angular velocity and measurement noise lie within 
given ellipsoidal bounds, an uncertainty ellipsoid that bounds the attitude and the angu- 
lar velocity of the rigid body is obtained. The center of the uncertainty ellipsoid provides 
point estimates, and the size of the uncertainty ellipsoid measures the accuracy of the esti- 
mates. The point estimates and the uncertainty ellipsoids are propagated using a Lie group 
variational integrator and its linearization, respectively. The attitude and angular velocity 
estimates are optimal in the sense that the sizes of the uncertainty ellipsoids are minimized. 
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1 Introduction 



Attitude estimation is often a prerequisite for controlling aerospace and underwa- 
ter vehicles, mobile robots, and other mechanical systems moving in space. In this 
paper, we study the attitude estimation problem for the uncontrolled dynamics of 
a rigid body in an attitude-dependent force potential (like uniform gravity). The 
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estimation scheme we present has the following important features: (1) the attitude 
is globally represented without using any coordinate system, (2) the filter obtained 
is not a Kalman or extended Kalman filter, and (3) the attitude and angular velocity 
measurement errors are assumed to be bounded, with known ellipsoidal uncertainty 
bounds. The static attitude estimation scheme presented here is based on [1]. The 
attitude is represented globally using proper orthogonal matrices and the exponen- 
tial map on the set of 3 x 3 skew-symmetric matrices. Such a global representation 
has been recently used for partial attitude estimation with a linear dynamics model 
in [2]. The estimation scheme presented here is deterministic, based on known mea- 
surement uncertainty bounds propagated by the dynamic flow. 

The attitude determination problem for a rigid body from vector measurements 
was first posed in [3]. A sample of the literature in spacecraft attitude estimation 
can be found in [4-8]. Applications of attitude estimation to unmanned vehicles 
and robots can be found in [2,9-1 1]. Most existing attitude estimation schemes use 
generalized coordinate representations of the attitude. As is well known, minimal 
coordinate representations of the rotation group, like Euler angles, Rodrigues pa- 
rameters, and modified Rodrigues parameters (see [12]), usually lead to geometric 
or kinematic singularities. Non-minimal coordinate representations, like the unit 
quaternions used in the quaternion estimation (QUEST) algorithm [7] and its sev- 
eral variants [4,8,13], have their own associated problems. Besides the extra con- 
straint of unit norm that one needs to impose, the quaternion vector itself can be 
defined in one of two ways, depending on the sense of rotation used to define the 
principal angle. 

A brief outline of this paper is given here. In Section 2, the attitude determina- 
tion problem for vector measurements with measurement noise is introduced, and 
a global attitude determination algorithm which minimizes the attitude estimation 
error is presented. The attitude dynamics and dynamic estimation problem is for- 
mulated for a rigid body in an attitude-dependent potential. Section 3 presents the 
attitude estimation scheme assuming that both attitude and angular velocity mea- 
surements are available simultaneously. Sufficient conditions for convergence of 
the estimates are given. This attitude estimation scheme has also been extended 
and applied to the case where only attitude but no angular velocity measurements 
are available, and recently reported in [14]. Section 4 presents concluding remarks 
and observations. 



2 Attitude estimation from vector observations 

Attitude of a rigid body is defined as the orientation of a body fixed frame with 
respect to an inertial reference frame; it is represented by a rotation matrix that is 
a 3 X 3 orthogonal matrix with determinant +1. Rotation matrices have a group 
structure denoted by SO (3), and its action on M.^ takes a vector represented in body 
fixed frame into its representation in the reference frame by matrix multiplication. 
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2. 1 Attitude determination procedure 



We assume that there are m fixed points in the spatial reference frame, no two of 
which are co-linear, that are measured in the body frame. We denote the known 
direction of the ith point in the spatial reference frame as e* e S^, and the cor- 
responding vector represented in the body fixed frame as U e Since we only 
measure directions, we normalize and U so that they have unit lengths. The 
and If are related by a rotation matrix C e SO (3) that defines the attitude of the 
rigid body; 

for all i e {1, 2, ■ ■ ■ , m}. We assume that e* is known accurately and we assume 
that 6* is measured in the body fixed frame. Let the measured direction vector be 
If e S^, which contains measurement errors, and let an estimate of the rotation 
matrix be C e SO (3). The vector estimation errors are given by the 

- CU, i ^ 1, . . . ,m. 



The attitude determination problem consists of finding C e SO (3) such that the 
weighted 2 norm of these errors is minimized: 



c 2 



1 rr 

1 



= ^tr \{E - CBfw{E - CB)] , (1) 
subject to C e SO (3), 



nSxm 



and W 



where £; = [e\ e^, ■ ■ ■ , e™] G K^^™, B 
diag , w'^, ■ ■ ■ , w"^] E has a weighting factor for each measurement. We 

assume that m > 3 in this paper. If m = 2, we can take the cross-product of the 
two measured unit vectors b^ xb"^ = b^ and treat that as a third measured direction; 
the corresponding unit vector in the inertial frame is — x e^. 

The problem (1) is known as Wahba's problem [3]. The original solution of Wahba's 
problem is given in [15], and a solution expressed in terms of quaternions, known 
as the QUEST algorithm, is presented in [7]. A solution without using generalized 
attitude coordinates is given in [1]. A necessary condition for optimality of (1) is 
given in [1] as 

L^C = C^L, (2) 

where L = EWB^ e R=^^=^. 
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The following result, proved in [1], gives an unique estimate C E SO (3) of the 
attitude matrix that satisfies (2) and solves the attitude determination problem (1). 

Theorem 2.1 The unique minimizing solution to the attitude determination prob- 
lem (1) is given by 

C^SL, S = Q^{RR^)-^Q^, (3) 

where 

L^QR, QeS0(3), (4) 

and R is upper triangular and invertible; this is the QR decomposition of L. The 
symmetric positive definite (principal) square root is used in (3). 

2.2 State bounding estimation 

Here we discuss the general idea of deterministic state bounding estimation, using 
ellipsoidal sets to describe state uncertainty and measurement noise. A stochastic 
state estimator requires probabilistic models for the state uncertainty and the noise. 
However, statistical properties of the uncertainty and the noise are often not avail- 
able. We usually make statistical assumptions on disturbance and noise in order to 
make the estimation problem mathematically tractable. In many practical situations 
such idealized assumptions are not appropriate, and this may cause poor estimation 
performance [16]. 

An alternative deterministic approach is to specify bounds on the uncertainty and 
the measurement noise without any assumption on its distribution. Noise bounds 
are available in many cases, and deterministic estimation is robust to the noise 
distribution. An efficient but flexible way to describe the bounds is using ellipsoidal 
sets, referred to as uncertainty ellipsoids. 

The deterministic estimation process using uncertainty ellipsoids has the same 
structure as the Kalman filter. We assume that the initial state lies in a prescribed 
uncertainty ellipsoid. We propagate the initial uncertainty ellipsoid through time 
using the equations of motion. This defines a prediction step. After measuring the 
state, we find a bound on the state assuming that measurement error is bounded. 
The bound is described by a measurement uncertainty ellipsoid. We then obtain a 
minimal ellipsoid that contains the intersection of the predicted uncertainty ellip- 
soid and the measured uncertainty ellipsoid. This procedure is repeated whenever 
new measurements are available. 

This deterministic estimation procedure is illustrated in Fig. 1, where the left fig- 
ure shows evolution of an uncertainty ellipsoid in time, and the right figure shows a 
cross section at a fixed time when the state is measured. Suppose that the time inter- 
val between two sets of measurements is divided into / equal time steps for discrete 
integration, and the subscript k denotes the A;th discrete integration time step. At 
the previous measurement instant, corresponding to the kth time step, the state is 
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(a) Propagation of uncertainty ellip- 
soid 



(b) Filtering procedure 



Fig. 1 . Uncertainty ellipsoids 

bounded by an uncertainty ellipsoid centered at the estimated state Xk- This initial 
ellipsoid is propagated through time. Depending on the dynamics of the system, 
the size and the shape of the tube are changed. The new set of measurements are 
taken at the {k + /)th time step. At the (A; + /)th time step, the predicted uncertainty 
ellipsoid is centered at x^^-^. Another ellipsoidal bound on the state is obtained from 
the measurements. The measured uncertainty ellipsoid is centered at x^^. The state 
lies in the intersection of the two ellipsoids. In the estimation procedure, we find a 
new ellipsoid that contains the intersection, which is shown in the right figure. The 
center of the new ellipsoid, Xk+i is considered as a point estimate at time step k + l, 
and the magnitude of the new uncertainty ellipsoid measures the accuracy of the 
estimation. If the size of the uncertainty ellipsoid is small, then we can conclude 
that the estimated state is accurate. The deterministic estimation is optimal in the 
sense that the size of the new ellipsoid is minimized. 

The deterministic estimation process is based on the state estimation techniques 
developed in [ 1 7] . Optimal deterministic state or parameter estimation is considered 
in [18-20], where an analytic solution for the minimum ellipsoid that contains a 
union or an intersection of ellipsoids is given. Parameter estimation in the presence 
of bounded noise is dealt with in [21,22]. 

2.3 Attitude Estimation Problem formulation 
2.3.1 Equations of motion 

We consider estimation of the attitude dynamics of a rigid body in the presence of 
an attitude dependent potential. We assume that the potential U (•) : S0(3) t— > M is 
determined by the attitude of the rigid body, C E SO (3). A spacecraft on a circular 
orbit including gravity gradient effects [23], or a 3D pendulum [24] can be modeled 
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in this way. The continuous equations of motion are given by 

Ju + CO X Jtu = M, (5) 
C = CS{lj), (6) 

where J e R^^^ is the moment of inertia matrix of the rigid body, a; e is the 
angular velocity of the body expressed in the body fixed frame, and S{-) : ^— > 
50 (3) is a skew mapping defined such that S{x)y = xxy for all x, y E R'^. M G 
is the moment due to the potential. The moment is determined by the relationship, 

S{M) = fg^C - C^^, or more explicitly, 

M ^ ri X + r2 X + ra X Vr^, (7) 

where rj, w^. G M^^'^ are the ith row vectors of C and respectively. The detailed 
description of this rigid body model and the derivation of the above equations can 
be found in [24]. 

General numerical integration methods, including the popular Runge-Kutta schemes, 
typically preserve neither first integrals nor the characteristics of the configuration 
space, SO (3). In particular, the orthogonal structure of the rotation matrices is not 
preserved numerically. It is often proposed to parameterize (6) by Euler angles or 
quaternions instead of integrating (6) directly. However, Euler angles have singular- 
ities. The numerical simulation process has to be monitored and switching between 
Euler angle charts is necessary in order to avoid singularities. Quaternions are free 
of singularities, but the quaternion representing the attitude is required to have unit 
length. The matrix corresponding to a quaternion which is not of unit length is not 
orthogonal, and hence does not represent a rotation. 

To resolve these problems, a Lie group variational integrator for the attitude dy- 
namics of a rigid body is proposed in [24]. This Lie group variational integrator is 
described by the discrete time equations. 

hSiJcok + ^Mk) = FkJd - JdF^, (8) 
Cfc+i = CkFk, (9) 
Ja;fc+i = F'^JuJk + ^F'^Mk + ^M^+i , (10) 

where J a G is a nonstandard moment of inertia matrix defined by J,^ = ^tr [ J] /sxs— 
J, and Ffc G SO (3) is the relative attitude over an integration step. The constant 
/i G M"*" is the integration step size. This integrator yields a map {Ck,u;k) i— > 
(Cfc+i, ojfc+i) by solving (8) to obtain F/^. G S0(3) and substituting it into (9) and 
(10) to obtain Ck+i and tOk+i- Numerically, we ensure that F^ remains on S0(3) 
by requiring that F^ = exp{S{fk)), where G M^. This allows us to express the 
discrete equations in terms of fk G M.^ as opposed to Fk G SO (3). 
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Since this integrator does not use a local parameterization, the attitude is defined 
globally without singularities. It preserves the orthogonal structure of S0(3) be- 
cause the rotation matrix is updated by a multiplication of two rotation matrices in 
(9), which is a group operation of S0(3). This integrator is obtained from a discrete 
variational principle, and it exhibits the characteristic symplectic and momentum 
preservation properties, and good energy behavior characteristic of variational in- 
tegrators. We use (8), (9), and (10) in the following development of the attitude 
estimator. 



2.3.2 Uncertainty Ellipsoid 

An uncertainty ellipsoid in is defined as 



X e 



xfp-^{x-x)<l^, (11) 



where x e R", and P e R"^" is a symmetric positive definite matrix. We call x 

the center of the uncertainty ellipsoid, and we call P the uncertainty matrix that 
determines the size and the shape of the uncertainty ellipsoid. The size of an uncer- 
tainty ellipsoid is measured by tr [P] . It equals the sum of the squares of the semi 
principal axes of the ellipsoid. 



The configuration space of the attitude dynamics is SO (3), so the state evolves 
in the 6 dimensional tangent bundle, TS0(3). Thus the corresponding uncertainty 
ellipsoid is a submanifold of TS0(3). An uncertainty ellipsoid centered at ((7, Qj) 
is induced from an uncertainty ellipsoid in R^, using the Lie algebra so (3); 



8{C,uj,P) = {C e S0(3), Lo e 



C 



e^R6(06,P) 



(12) 



where S{C) = logm [C^C) e so(3), 6uj = uj - uj e R\ and P e 

symmetric positive definite matrix. Equivalently, an element (C, lu) G S{C, uj, P) 
can be written as 

C = C'exp(5(C)), 
uj — u) -\- Sco, 



p6x6 ; 



IS a 



for some x — [(^ , ScJ^]^ e R^ satisfying x^P ^x < 1. 



2.3.3 Uncertainty model 

We describe the measurement error models for the measured direction vectors and 
the angular velocity. The direction vector U E is measured in the body fixed 
frame, and let 6* e denote the measured direction. Since we only measure direc- 
tions, we normalize and If so that they have unit lengths. Therefore it is inappro- 
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priate to express the measurement error by a vector difference. The measurement 
error is modeled by rotation of the measured direction; 



b' = exp {S{u')y¥ 
~ 6* + S{i^')b\ 



(13) 



where e is the measurement error, which represents the Euler axis of rotation 
vector from 6* to b\ and ||z/'|| is the corresponding rotation angle in radians. We 
assume that the measurement error is small to obtain the second equality. 

The angular velocity measurement errors are modeled as 



where G is the measured angular velocity, and f e is an additive mea- 
surement error. 

We assume that the initial conditions and the measurement error are bounded by 
prescribed uncertainty ellipsoids. 



where Pq G K^^*", and S\ T G M^^^ are symmetric positive definite matrices that 
define the shape and the size of the uncertainty ellipsoids. 



3 Attitude Estimation with Angular Velocity Measurements 

In this section, we develop a deterministic estimator for the attitude and the an- 
gular velocity of a rigid body assuming that both the attitude measurement and 
the angular velocity measurements are available. The estimation process consists 
of three stages; flow update, measurement update, and filtering. The flow update 
predicts the uncertainty ellipsoid in the future. The measurement update finds an 
uncertainty ellipsoid in the state space using the measurements and the measure- 
ment error model. The filtering stage obtains a new uncertainty ellipsoid compatible 
with the predicted uncertainty ellipsoid and the measured uncertainty ellipsoid. 

The superscript i denotes the ith directional measurement. The superscript / de- 
notes variables related to the flow update, while the superscript m denotes variables 
related to the measurement update. The notation ~ denotes a measured variable, 
while • denotes an estimated variable. 



U) — cij + V 



(14) 



(15) 

(16) 
(17) 
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3.1 Flow update 



Suppose that the attitude and the angular momentum at the A;th step, which corre- 
sponds to the previous measurement instant, lie in a given uncertainty ellipsoid: 

{Ck,tOk) £ ^{Ck,l^k, Pk)- 

The flow update gives us the center and the uncertainty matrix that define the un- 
certainty ellipsoid at the (/c + /)th step (the current measurement instant) using the 
given uncertainty ellipsoid at the A;th step. Since the attitude dynamics of a rigid 
body is nonlinear, the boundary of the state at the (A; + /)th step is not an ellip- 
soid in general. We assume that the given uncertainty ellipsoid at the A;th step is 
sufficiently small that the states in the uncertainty ellipsoids can be approximated 
by linearized equations of motion. Then we can guarantee that the boundary of the 
state at the {k + /)th step is an ellipsoid, and we can compute the center and the 
uncertainty matrix at the (A; + /)th step separately. 

Center: For the given center, {Ck, uJk), the center of the uncertainty ellipsoid due to 
flow propagation is denoted {CI_^_i,ujI_^_i). This center is obtained from the discrete 
equations of motion, (8), (9), and (10) applied to {Ck, cDfe): 

hS{JcOk + ^Mk) = hJd - JdPj, (18) 
Cl+, = CkFk, (19) 
M+i = F^J<^k + ^F^Mk + ^Mfe+i. (20) 

This integrator yields a map {Ck,Cjk) ^ {CI^i,ujI_^_i), and this process can be 
repeated to find the center at the (A; + /)th step, {C[_^i, i^l+i)- 

Uncertainty matrix: We assume that an uncertainty ellipsoid contains small pertur- 
bations from its center. Then the uncertainty matrix is obtained by linearizing the 
above discrete equations of motion. At the {k + l)th step, the uncertainty ellipsoid 
is represented by perturbations from the center {Cl_^i, as 

C,+i = (7/_,i exp (5(C/+i)), 

for some Cfe+i; ^^l+i ^ The uncertainty matrix at the {k + l)th step is obtained 
by finding a bound on Cl+i^ ^^l+i ^ Assume that the uncertainty ellipsoid at 
the kth step is sufficiently small. Then, Cf+i , <^^k+i represented by the following 
linear equations (using the results presented in [23]): 

^k+l — -^k^k. 
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where = [Cj,duj^]^ e MP, and AI E M®^^ can be suitably defined. Since 
(Cfc, ujk) E S{Ck, Cjk, Pk), Xk E i'Re(0, Pk) by the definition of the uncertainty el- 
lipsoid given in (12). Then we can show that A^Xk lies in the following uncertainty 
ellipsoid. 

AixkE8^,{^,AiPk{Aify 
Thus, the uncertainty matrix at the (A; + l)th step is given by 

PU-<Pk{Alf ■ (21) 

The above equation can be applied repeatedly to find the uncertainty matrix at the 
(A; + /)th step. 

We have obtained expressions to predict the center and the uncertainty matrix in the 
future from the current uncertainty ellipsoid using the discrete flow. In summary, 
the uncertainty ellipsoid at the {k + 0th step is computed using (18), (19), (20), and 
(21) as: 

{Ck^i,u^k^i) e £{Cl^i, ul^i, pU, Pl^i = AfPk{Al)^, {IT) 
where A^ = a1^i_X+i-2 ■ ■ ■ 4 ^ R'"'- 



3.2 Measurement update 



We assume that the attitude and the angular velocity of a rigid body are measured 
simultaneously. The measured attitude and the measured angular velocity have un- 
certainties since the measurements contain measurement errors. However, we can 
find bounds for the actual state because the measurement errors are bounded by 
known uncertainty ellipsoids given by (16) and (17). The measurement update stage 
finds an uncertainty ellipsoid in the state space using the measurements and the 
measurement error models. The measured attitude and the measured angular veloc- 
ity are the center of the measured uncertainty ellipsoid, and the measurement error 
models are used to find the uncertainty matrix. 

Center: The center of the uncertainty ellipsoid, (C'^^.cj^^J is obtained by mea- 
surements. The attitude is determined by measuring the directions to the known 
points in the inertial frame. Let the measured directions to the known points be 
Bk+i = &^ • • • , e R^^"*. Then, the attitude (7^^ satisfies the following 
necessary condition given in (2): 

ipk+i) ^k+i - Lk+fik+i = 0> (23) 
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where Lk+i = Ek+iWk+iBl_^i G M^""^. The solution of (23) is obtained by a QR 
factorization of L^+i as given in Theorem 2. 1 

C^r+; = [Q<i\l{QrQ^)-'Ql) Lk+i, (24) 

where Qg G S0(3) is an orthogonal matrix and Qr G M^^^ is a upper triangular 
matrix satisfying Lk+i = QqQr- 

The angular velocity is measured directly, 

^r+z = (25) 



Uncertainty matrix: We can represent the actual state at the {k + /)th step using the 
measured center and perturbations as follows. 

C,+, = C'r+,exp(5(0), (26) 
a;fc+, = (^-; + ^<+z, (27) 

for (5c<j^^ G M^. The uncertainty matrix is obtained by finding an ellipsoid 
containing C*?V;,5wr+z- 

We determine the attitude indirectly by comparing the known directions in the ref- 
erence frame with measurements in the body frame. So, we need to transform the 
uncertainties in the direction measurements into the uncertainties in the rotation 
matrix by (23). Using the measurement error model defined in (13), the actual di- 
rection matrix to the known point Bk+i is given by 

Bk+i — Bk+i + 5Bk+i, (28) 

where 6Bk+i = [S{u^)b\ S{v^)P, ■■■ , 

The actual matrix giving the known directions Bk+i and the actual attitude Ck+i at 
the {k + l)th step also satisfy (24); 

cl^iLk+i - hlj^fik+i = 0, (29) 

where Lk+i = Ek+iWk+iB^^i G R^""^. Substituting (26) and (28) into (29), and 
assuming that the size of the measurement error is sufficiently small, the above 
equation can be written as 

Lk+iC'k+i^iCk+i) + ^iCk+i) (fi^+i) Lk+i = (C'fc+/) Ek+iWk+iSB^^i - dBk+iWk+iE^^iCl!':^i. 
Using the identity, S{x)A + A^S{x) = S{{tr [A] I^^s - A} x) for A G R^^^ x G 
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the above equation can be written in a vector form. 



tr 



T 1 T 1 ^ 

■' i=i 



m 
k+l 



Then, we obtain 



^k+l — / .•^k+jy 1 



(30) 



where 



(31) 



This equation expresses the uncertainty in the measured attitude as a linear combi- 
nation of the directional measurement errors. 

The perturbation of the angular velocity Scu^^i is equal to the angular velocity mea- 
surement error Vk+i, since we measure the angular velocity directly. Substituting 
(27) into (14), we obtain 



SuJk+i = Vk+i- 



(32) 



Define x^^i = [{Cr+if, (Sco^^+i)^]^ e Using (30) and (32), 

m 



i=l 



where Hi = [/sxs, Osxs] , -f^2 = [Osxs, hxs] e K'^''^. Now xf^i is expressed 
as a linear combination of the measurement errors v'^ and v. Using (16) and (17), 
we can show that each term in the right hand side of the above equation is in the 
following uncertainty ellipsoids. 



H2Vk+i e (O, H2Tk+iHj^ . 

Thus, the uncertainty ellipsoid for is obtained as the vector sum of the above 
uncertainty ellipsoids. The measurement update procedure is to find a minimal el- 
lipsoid that contains the vector sum of those uncertainty ellipsoids. Expressions for 
a minimal ellipsoid containing the vector sum of multiple ellipsoids are presented 
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in [18] and [19]. Using the results, we obtain 



^k+l 



+ A tr 



X < 



E 

i=l 



T 

TT Am,i Qi ( i\m,i\ ttT 



+ 



H2Tk+iHj 
H2Tk+iHj 



tr 



H2Tk+iHj 



■ (33) 



In summary, the measured uncertainty ellipsoid at the {k + /)th step is defined by 
(24), (25), and (33); 

{Ck+i,u;k+i) e £{C^^i,u;'^^i, P^i). (34) 



3.3 Filtering procedure 



The filtering procedure is to find a new uncertainty ellipsoid compatible with both 

the predicted uncertainty ellipsoid and the measured uncertainty ellipsoid. From 
(22) and (34), we know that the state at {k + /)th step lies in the intersection of the 
two uncertainty ellipsoids: 

{Ck+hLOk+i) e ^{Cl+i, i^k+u H+i) n ^i^k+b '^k+b Pk+i)- (35) 

However, the intersection of two ellipsoids is not generally an ellipsoid. We find 
a minimal uncertainty ellipsoid containing this intersection. We first obtain equiv- 
alent uncertainty ellipsoids in M*^, and convert them to uncertainty ellipsoids in 
TS0(3). We omit the subscript {k + /) in this subsection for convenience. 

The uncertainty ellipsoid obtained from the measurements, £{C'^^ co"^, P"^), is iden- 
tified by its center {C"^, a)™), and the uncertainty ellipsoid in MP: 

(r,5u;^)eE^e(0e^,,P^), (36) 

where 5(C'") = logm (c^^'^c) e 5o(3), Sco"' ^ to - u"^ e R^. Similarly, the 

uncertainty ellipsoid obtained from the flow update, £{C^, u;^, P^), is identified by 
its center [C^ ,Cj^), and the uncertainty ellipsoid in M^. 

(C^5a;0e^R6(06xi,P0, (37) 
where S{C,f) = logm (C-^-^C) e so(3), Sujf ^ u - Cb^ e Equivalently, an 
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element {C^ , ujf) e £{Cf, u^, Pf), is given by 

Cf = Cfcxp(s{C^)), (38) 



UJ' 



f ^u^ + 5u^, (39) 



for some {C,^ ,5uj^) e £r6(06xi, ^^)- We find an equivalent expression for (37) 
based on the center (C*"*, a)™) obtained from the measurements. 

Define C™^, Suj'^f e such that 

= C"^ exp (^(C-^)) , (40) 
a;/ = cu™ + ^tu™^. (41) 

Thus, C,'^^ , 6cb"^^ represent the difference between the centers of the two ellipsoids. 
Substituting (40), (41) into (38), (39), we obtain 

Cf = C^^exp (5(r^)) exp (^(CO), 

^(7™exp(5(r^ + C0), (42) 
u;/ = cD"^ + (^cD"^-^ + Sujf) , (43) 

where we assumed that C™-^, C'^ are sufficiently small to obtain the second equality. 
Thus, the uncertainty ellipsoid obtained by the flow update, S{C-f , Cj^ , P f), is de- 
scribed by the center (C"*, a)"*) obtained from the measurement and the following 
uncertainty ellipsoid in R^: 

E^e{x"'f,pf), (44) 
where x"'^ = [(C'""'')'^, {Su;"'^)^]^ G M^. 

We seek a minimal ellipsoid that contains the intersection of two uncertainty ellip- 
soids in R^. 

^R6(06xi,P'")n^K«(^™^^0 C S^e{x,P), (45) 

where x — [(^, Sui^]^ e R^. Using the result presented in [18], x and P can be 
written as 

x^LSr^, P ^ (3{q){I - L)P'^, (46) 

where 

f3{q) = 1 + g - (x"^-^)T(P"^)-iLx"^/, (47) 
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The constant q is chosen such that tr [P] is minimized. We convert x to points in 
TS0(3) using the common center {C^^ tD'"). 

In summary, the attitude estimation filter algorithm is given by the following state- 
ment. 

Proposition 3.1 The attitude and angular velocity estimates and the new uncer- 
tainty ellipsoid at the {k + l)th step are given by 



where x = [(^ , 6ui^]^ e and P e M*^""^ are given by equations (46)-(48). The 
actual state lies in the ellipsoid 



centered at the estimated attitude and angular velocity states. 

The center of the new uncertainty ellipsoid is the estimated state, considered as 
point estimates of the attitude and the angular velocity at the (A; + /)th step. The un- 
certainty matrix represents the bounds on the uncertainty of the estimated state. The 
size of the uncertainty matrix characterizes the accuracy of the estimate. If the size 
of the uncertainty ellipsoid is small, we conclude that the estimation is accurate. 
This estimation is optimal in the sense that the size of the new uncertainty ellipsoid 
is minimized. The uncertainty matrix can also be used to predict the distribution 
of the uncertainty. The eigenvector of the uncertainty matrix corresponding to the 
maximum eigenvalue shows the direction of the maximum uncertainty. 

3.4 Convergence of Filter 

We now present a sufficient condition under which this estimation algorithm con- 
verges, i.e., the size of the uncertainty matrix decreases monotonically with mea- 
surements. The trace of the positive definite uncertainty matrix P is the measure of 
size used in this analysis. 

Theorem 3.1 Let A = diag[Aj], i — 1, . . . be the matrix of real positive eigen- 
values of{P"^)~^Pf and let Amm be the smallest of the Aj. The estimation algorithm 
given by Proposition 3.1 is convergent if there exists a constant c e (0, 1) such that 
the following inequality holds for every measurement, 




(49) 

(50) 
(51) 



Pk+l — P, 



{Ck+U<^k+l) G ^{Ck+U'^k+uPk+l)^ 



(52) 



ll^^ll < 



C{q + Amin) 



(53) 



^6x(P-)(l + g) 
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where \\ ■ \\ denotes the Frobenius norm, is the linear discrete flow from the 
previous to the current measurement instant, and 

X(P™) = V6 + 30«:(P-), 
k{P"^) is the condition number of P^. 

Proof. For convergence, it is sufficient that the filtering process is a contraction 
mapping, which is to say that tr[P] < ctr [Pq] where c e (0, 1) and Pq denotes 

the uncertainty matrix of the filtered estimate at the previous measurement in- 
stant. The logic of this proof is as follows. We first obtain a real- valued function 
G'(P™,P^,g), such that 



tr[P]<||A^||2G(P-,P^,g)tr[Po], 



where 11 A I 



tr 



denotes the Frobenius norm. Our sufficient condition 



G(P- P/,g)' 



for convergence can then be stated as, 
This implies that 

\\AffG{P"^,pf,q)tr[Po]<ctr[Po], 
which is to say that the filter is a contraction mapping. 

Using the matrix inversion lemma, we can express the uncertainty matrix P given 
by (46)-(48) as 

P^i3iq)(qiPf)-' + iP"r'^ 

Now we have 



(q{p^)-' + {pn~') ' 



tr[P] = /?(g)tr 
^From equations (47)-(48), we have 

(3{q) <l + q. 

^From Lemma A.l in Appendix A, we know that (P"*)~^P-^ can be diagonalized 
as 

where A = diag[Ai], i = 1, . . . , 6 is a positive diagonal matrix, i.e., Aj G M"*". Then 
the uncertainty matrix P is given by 



P^ P{q)PnqI+{P"')-^pf 



P{q)pfu(ql + A] U-^ 



(54) 
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We use the above expressions for the uncertainty matrix P and /3(g), the Cauchy- 
Schwartz inequality for the Frobenius norm of matrices [25], and the fact that 
is a symmetric positive definite matrix, to obtain: 

tr [P] < (1 + g)tr [P^U{qI + Ky^U'^' 

< (1 + g)tr [{Pff] ^ \\U{qI + K)'^U'^ 



< (1 + g)tr 

< (1 + g)tr 



pf 
pf 



UWWiql + A)-^\\\\U-^\\ 
[/-^tr f(g/ + A)-^ 



The third step is due to the Frobenius norm being submultiplicative, as it is equal to 
the Holder 2-norm [26]. From the proof of Lemma A.l, we see that;7 = Q'^(P"^)5 
where Q is an orthogonal matrix. Therefore, we have 



\\U\\=tr 



tr[P" 



\U 



-1 1 



tr 



tr 



m\—l 



Let (Ti > • • • > (76 > denote the eigenvalues of P"* in descending order. Then 

1 



m\ — l 



tr[P'"]tr (P™) 



(ai + £72 + . . . + (Te) ( — + — + 



+ 



(7i (72 



,^1 



< 6 + 6(5)— = 6 + 30k(P"*) = x iP""), 



where n{P'^) = ^ is the condition number of P™. Therefore, we now have 



tr [P] < (1 + g)tr [{A^fA^Po] x(P"^)tr [{ql + A)"^ 



< (l + g)tr 



tr 



p2 



x(P"^)tr[(g/ + A)-i 



< 
< 



(1 + P^ir X(0tr [(?/ + A)-^] tr [Po] . 
^^^^P^ftrtPo] 



where, in the last inequality, we used the the fact that tr[A ^] < ^ ^^^^ when 

A e R^^^ is positive definite and ainin(^) is the minimum eigenvalue of A. Using 
the above inequality, we can let 

q ~\~ Amin 

Our initial discussion then implies that 

\\A^\ < 



C{q + Amin) 

G(Pr-,Pf,q)~\6x(P"'){l + q) 
is a sufficient condition for convergence. □ 
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The rate of convergence is determined by the contraction constant c G (0, 1) for 
which the inequality (53) is satisfied for all measurements. Note that the bound 
depends on Amin =minimum eigenvalue of the ratio of the relative 

sizes of the uncertainty matrices due to the flow and the measurements. Since — 
Pq{A^)^ , a lower bound for Amin can be obtained in terms of ||74-^|| which when 
combined with the relation (53) yields an implicit bound on . This implicit bound 
is a sufficient condition for (53) to be satisfied. 

The sufficient condition (53) also depends on the condition number of P"*, which 
suggests that the ease of convergence of this scheme is increased if k{P^) is small, 
i.e., the measurement uncertainty bound is more spherical and less oblate. Also 
note that the size (norm) restriction on imposed by (53) becomes more stringent 
when Amin becomes smaller as size of the measurement uncertainty (given by P^) 
becomes larger. Thus, smaller measurement uncertainty also leads to easier conver- 
gence for the filter, as would be intuitively expected. Other sufficient conditions for 
convergence of the filter algorithm can be obtained using similar analysis. 



4 Conclusion 

A deterministic estimation scheme for the attitude dynamics of a rigid body in an 
attitude dependent potential field is presented, with an assumption of bounded mea- 
surement errors. The properties of the proposed attitude estimation scheme are as 
follows. This attitude estimator has no singularities since the attitude is represented 
by a rotation matrix, and the structure of the rotation matrix is preserved since it 
is updated by group operations in S0(3) using a Lie group variational integrator. 
The proposed attitude estimator is robust to the distribution of the uncertainty in 
initial conditions and the measurement noise, since it is a deterministic scheme 
based on knowledge of the bounds in these uncertainties. A sufficient condition for 
convergence of this filter has been obtained. 



A Product of two symmetric positive definite matrices 

Lemma A.l Suppose A,Be M"^" are symmetric positive definite. Then, there 
exists a nonsingular matrix V e R"^" and a diagonal matrix A = diag[Aj], i — 
1, . . . ,n such that Aj e IR"^ and 

AB = V-^AV. (A.l) 



Proof: Since A is symmetric positive definite, it can be diagonalized by a real or- 
thogonal matrix Qa such that A — Qa^aQa where A^ is diagonal with positive 
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real diagonal elements. The symmetric positive definite matrix square root of A is 

1 — T' 11 

A2 = Qj^A^Q^. Since the matrix A2BA2 is also symmetric and positive definite, 
it is diagonalized by a real orthogonal matrix Q such that 

A^BA^ = QAQ^, (A.2) 

where A is diagonal with positive real diagonal elements. Define a non-singular 
matrix V = Q^A~^ e M"^". Now consider 

VABV-^ = Q^A^/^BA^/'^Q = Q^QAQ^Q = A, 

which yields (A. 1). □ 

An alternate proof of the above statement is given in [26], while a more general 
result applicable to a product of two Hermitian matrices is given in [25]. 
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